#pragma once

#include <iostream>
#include <vector>
#include <memory>
#include <boost/circular_buffer.hpp>
#include "common_lib/libtime.h"

enum eBrakeCmd
{
    BRAKE = 0,
    UNBRAKE = 1
};

enum eRunningType
{
    HEADFOLLOWING = 0,
    ROTATING,
    STEERTURNING
};

enum eMoveMode
{
    STILL = 0,
    TOWARD,
    TURN,
    ROTATE,
    HEADFOLLOW
};

enum eMANUALORAUTO
{
    MANUAL,
    AUTO,
    REMOTER
};

struct SteerCmd
{
    float steer_speed;
    float steer_angle;
    bool  steer_init;
    eBrakeCmd brake_cmd;
    SteerCmd()
    {
        steer_speed = 0.0f;
        steer_angle = 0.0f;
        steer_init  = false;
        brake_cmd = eBrakeCmd::BRAKE;
    }
};

struct SteerState
{
    float steer_speed;
    float steer_angle;
    uint32_t steer_state;
    SteerState()
    {
        steer_speed = 0.0f;
        steer_angle = 0.0f;
        steer_state = 0x00;
    }
};

struct SteerParams
{
    float wheel_radius;
    float wheel_ratio;
    float steer_ratio;
    int8_t wheel_reverse; // 轮子是否反向
    int8_t steer_reverse; // 转向电机是否反向
    float pos_x;
    float pos_y;
    float steer_angle_offset;

    SteerParams()
    {
        wheel_reverse = 1;
        steer_reverse = 1;
        wheel_ratio = 1.0f;
        steer_ratio = 1.0f;
        steer_angle_offset = 0.0f;
    }
};

struct AMRReceiveState
{
    eMANUALORAUTO manual_auto;
    std::vector<struct SteerState> steer_state;
    std::vector<SteerParams> steer_param;
};

struct AMRSendCmd
{
    eMoveMode move_mode;
    std::vector<struct SteerCmd> steer_cmd;
};

struct AMRCmdAndState
{
    eMoveMode move_mode;
    eMANUALORAUTO manual_auto;
    std::vector<struct SteerCmd> steer_cmd;
    std::vector<struct SteerState> steer_state;
    std::vector<SteerParams> steer_param;
};

struct st_par
{
    common_lib::LibTime time;
	int radar_type;			// 激光雷达内部编号
	int radar_upsidedown;	// 雷达是否倒立安装，0-正装，1-倒装
	int radar_qty;			// 激光雷达数量
	int radar_pos_x_1;		// 激光雷达1坐标X，mm
	int radar_pos_y_1;		// 激光雷达1坐标Y，mm
	float radar_pos_z_1;	// 激光雷达1角度，°
	int radar_pos_x_2;		// 激光雷达2坐标X，mm
	int radar_pos_y_2;		// 激光雷达2坐标Y，mm
	float radar_pos_z_2;	// 激光雷达2角度，°
	int radar_pos_x_3;		// 激光雷达3坐标X，mm
	int radar_pos_y_3;		// 激光雷达3坐标Y，mm
	float radar_pos_z_3;	// 激光雷达3角度，°
	int radar_pos_x_4;		// 激光雷达4坐标X，mm
	int radar_pos_y_4;		// 激光雷达4坐标Y，mm
	float radar_pos_z_4;	// 激光雷达4角度，°
	int imu_pos_x;			// 控制器安装位置坐标X，mm
	int imu_pos_y;			// 控制器安装位置坐标Y，mm
	int imu_pos_z;			// 控制器安装位置坐标Z，mm
	int imu_pos_dir;		// 控制器安装方向
	float odo_velocity_x;	// 里程计速度X分量，mm/s
	float odo_velocity_y;	// 里程计速度Y分量，mm/s
	float odo_velocity_z;	// 里程计角速度，°/s
	int odo_ts_low;			// 里程计时间戳低32位，ns
	int odo_ts_high;		// 里程计时间戳高32位，ns
	int imu_acc_x;			// IMU加速度X分量
	int imu_acc_y;			// IMU加速度Y分量
	int imu_acc_z;			// IMU加速度Z分量
	int imu_ang_x;			// IMU角加速度X轴
	int imu_ang_y;			// IMU角加速度Y轴
	int imu_ang_z;			// IMU角加速度Z轴
	int imu_ts_low;			// IMU时间戳低32位，ns
	int imu_ts_high;		// IMU时间戳高32位，ns
	int safty_forward_0;	// 雷达安全区域_0，前方距离，0-无效值，需输出对应区域停止信号，下同
	int safty_right_0;		// 雷达安全区域_0，右侧距离
	int safty_backword_0;	// 雷达安全区域_0，后方距离
	int safty_left_0;		// 雷达安全区域_0，左侧距离
	int safty_forward_1;	// 雷达安全区域_1，前方距离
	int safty_right_1;		// 雷达安全区域_1，右侧距离
	int safty_backword_1;	// 雷达安全区域_1，后方距离
	int safty_left_1;		// 雷达安全区域_1，左侧距离
	int slam_pos_x;			// SLAM定位X坐标，mm
	int slam_pos_y;			// SLAM定位Y坐标，mm
	float slam_pos_z;		// SLAM定位姿态角，°
	int slam_accuracy;		// SLAM置信度
	int slam_ts_low;		// SLAM定位时间戳低32位，ns
	int slam_ts_high;		// SLAM定位时间戳高32位，ns
	int safty_level;		// 雷达保护等级，0-区域0有障碍物，1-区域1有障碍物且区域0无障碍物，2-无障碍物
	int order_action;		// 目标动作号
	int action_return;		// 动作完成返回值
	int speed_sv_x;
	int speed_sv_y;
    float drv_pv_1;         // feadback
    float drv_pv_2;
    float drv_pv_3;
    float drv_pv_4;

    int rc_sv_x; // mm/s
    int rc_sv_y;
    float rc_sv_z; // 角速度 °/s
    bool b_auto_mode; // 自动情况下写，0，表示手动。

    float drv_sv_1;
    float drv_sv_2;
    float drv_sv_3;
    float drv_sv_4;
	float speed_sv_z;
};

